/*
 * @Description: 订阅激光点云，并解析数据
 * @Author: Sang Hao
 * @Date: 2021-09-15 17:10:53
 * @LastEditTime: 2021-10-27 14:19:49
 * @LastEditors: Sang Hao
 */

#include "glog/logging.h"

#include "lidar_slam/subscriber/cloud_subscriber.hpp"

namespace lidar_slam {

CloudSubscriber::CloudSubscriber(ros::NodeHandle& nh, std::string topic_name, size_t buff_size) 
	: nh_(nh) {
	// 调用回调函数，跟回调函数建立关联
	subscriber_ = nh_.subscribe(topic_name, buff_size, &CloudSubscriber::msg_callback, this);
}

/**
 * @description: 有了新数据，则插入到cloud_data_buff中
 * @param  {*}
 * @return {*}
 */
void CloudSubscriber::ParseData(std::deque<CloudData>& cloud_data_buff) {
	if (new_cloud_data_.size() > 0) {
		cloud_data_buff.insert(cloud_data_buff.end(), 
			new_cloud_data_.begin(), new_cloud_data_.end());
		new_cloud_data_.clear();
	}
}

/**
 * @description: 订阅的消息是sensor_msgs::PointCloud2，取该类型的ConstPtr作为输入参数
 * @param  {*}
 * @return {*}
 */
void CloudSubscriber::msg_callback(const sensor_msgs::PointCloud2::ConstPtr& cloud_msg_ptr){
	CloudData cloud_data;
	// 从ROSMsg中获得PointCloud，并添加到新点云队列中去
	cloud_data.time = cloud_msg_ptr->header.stamp.toSec();
	pcl::fromROSMsg(*cloud_msg_ptr, *(cloud_data.cloud_ptr));
	new_cloud_data_.push_back(cloud_data);
}
}